package org.usfirst.frc;
import edu.wpi.first.wpilibj.*;

public class IOThread implements Runnable 
{
private static double[][] joystickAxes;
private static double[][] joystickButtons;
private static double[] jaguarOutput;
private static double[] victorOutput;
private static boolean[] solenoidOutput;
private Jaguar jagFrontRight, jagFrontLeft, jagBackRight, jagBackLeft;
private Joystick joyLeft, joyRight, xboxControl;
private Solenoid openClaw, closeClaw;
private Victor joint1, joint2;

/**
Creates a new thread that will initialize the robot and set positions and values
for the robot based on input from the controller modules.
**/
public IOThread()
	{
        joystickAxes = new double[3][];
        joystickAxes[0] = new double[2];
        joystickAxes[1] = new double[2];
        joystickAxes[2] = new double[5];
        joystickButtons = new double[3][];
        jaguarOutput = new double[4];
        victorOutput = new double[2];
        solenoidOutput = new boolean[2];
        joyLeft = new Joystick(1);
        joyRight = new Joystick(2);
        xboxControl = new Joystick(3);
        jagFrontRight = new Jaguar(4, 2);
        jagBackRight = new Jaguar(4, 1);
        jagFrontLeft = new Jaguar(4, 3);
        jagBackLeft = new Jaguar(4, 8);
        joint1 = new Victor(5);
        joint2 = new Victor(6);
        openClaw = new Solenoid(8,1);
        closeClaw = new Solenoid(8,2);
        new Thread(this).start();
	}

public static double getJoyAxes(int joy, int index)
    {
    return joystickAxes[joy][index];    
    }

public static double getJoyButtons(int joy, int index)
    {
    return joystickButtons[joy][index];    
    }

public static void setJagOut(int index, double val)
    {
    jaguarOutput[index] = val;    
    }

public static void setVictorOut(int index, double val)
    {
    victorOutput[index] = val;    
    }

public static void setSolenoidOut(int index, boolean val)
    {
    solenoidOutput[index] = val;    
    }

public void run()
{
while(true)
	{
	joystickAxes[0][0] = joyLeft.getX();
	joystickAxes[0][1] = joyLeft.getY();
	joystickAxes[1][0] = joyRight.getX();
	joystickAxes[1][1] = joyRight.getY();
	joystickAxes[2][0] = xboxControl.getRawAxis(1);
	joystickAxes[2][1] = xboxControl.getRawAxis(2);
	joystickAxes[2][2] = xboxControl.getRawAxis(3);
	joystickAxes[2][3] = xboxControl.getRawAxis(4);
	joystickAxes[2][4] = xboxControl.getRawAxis(5);
	jagBackLeft.set(joystickAxes[0][1]);
	jagBackRight.set(-joystickAxes[1][1]);
	jagFrontLeft.set(joystickAxes[0][1]);
	jagFrontRight.set(-joystickAxes[1][1]);
	joint1.set(victorOutput[0]);
	joint2.set(victorOutput[1]);
	openClaw.set(solenoidOutput[0]);
	closeClaw.set(solenoidOutput[1]);
    }
}
}